#include "PclViewTools.h"
#include "opencv2/rgbd.hpp"
#include <pcl/PolygonMesh.h>
// #include <pcl_io>
PclViewTools::PclViewTools(void)
{
}

PclViewTools::~PclViewTools()
{
}
/**
 * 把深度图 map 根据相机的内参 转换为 pcd 点云序列图 
 */
int PclViewTools::DepthToCloudPCD(const cv::Mat &rgb , const cv::Mat &depth, const string &savePath)
{
	// 读取./data/rgb.png和./data/depth.png，并转化为点云
	bool flag = false;
	if( !rgb.empty() ){
		flag = true;
	}
	// 图像矩阵
	// cv::Mat rgb, depth;
	// 使用cv::imread()来读取图像
	// rgb = cv::imread("F:/depth.png");
	assert(rgb.rows == depth.rows && rgb.cols == depth.cols);
	// rgb 图像是8UC3的彩色图像
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
	// depth = cv::imread("F:/depth.png", -1);
	assert(depth.empty() == false);
	assert(depth.channels() == 1 && depth.depth() == CV_16U);
	// 点云变量
	// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
	PointCloud::Ptr cloud(new PointCloud);

	// 遍历深度图
	for (int m = 0; m < depth.rows; m++)
		for (int n = 0; n < depth.cols; n++)
		{
			// 获取深度图中(m,n)处的值
			ushort d = depth.ptr<ushort>(m)[n];
			// d 可能没有值，若如此，跳过此点
			if (d == 0)
				continue;
			// d 存在值，则向点云增加一个点
			PointT p;

			// 计算这个点的空间坐标
			p.z = double(d) / camera_factor;
			p.x = (n - camera_cx) * p.z / camera_fx;
			p.y = (m - camera_cy) * p.z / camera_fy;

			if( flag){
				// 从rgb图像中获取它的颜色
				// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
				p.b = rgb.ptr<uchar>(m)[n * 3];
				p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
				p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
			}else{
				p.b = 255;
				p.g = 0;
				p.r = 0;
			}

	
			// 把p加入到点云中
			cloud->points.push_back(p);
		}

	// cv::Mat_<cv::Vec3f> depth_real_ref_raw;
	// cv::Mat_<float> K, K_depth_;
	// cv::Mat_<double> tmpMat_(3, 3);
	// tmpMat_ << 527.6911, 0, 478.74231, 0, 528.01282, 266.14407, 0, 0, 1;
	// K_depth_ = tmpMat_;
	// K_depth_.convertTo(K, CV_32F);
	// //IDebug("%s","detector ready ...............");
	// cv::rgbd::depthTo3d(depth, K, depth_real_ref_raw);
	// FileStorage fs("/home/de/fileName.xml" , FileStorage::WRITE);
	// fs << "mat" << depth_real_ref_raw;

	// 设置并保存点云   无序点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	cout << "point cloud size = " << cloud->points.size() << endl;
	cloud->is_dense = false;

	pcl::io::savePCDFile(savePath, *cloud);

	// 清除数据并退出
	cloud->points.clear();
	cout << "Point cloud saved." << endl;
	return 0;
}


int PclViewTools::DepthToCloud(const cv::Mat &rgb , const cv::Mat &depth, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud )
{
	// 读取./data/rgb.png和./data/depth.png，并转化为点云
	bool flag = false;
	if( !rgb.empty() ){
		cout << "point color " <<std::endl;
		flag = true;
	}
	cloud.get()->points.clear();

	assert(rgb.rows == depth.rows && rgb.cols == depth.cols);
	// rgb 图像是8UC3的彩色图像
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
	assert(depth.empty() == false);
	assert(depth.channels() == 1 && depth.depth() == CV_16U);
	// std::cout << "rgb is "<< rgb.channels()<<" "<<rgb.depth()<<endl;
	// 点云变量
	// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
	// PointCloud::Ptr cloud(new PointCloud);

	// 遍历深度图
	for (int m = 0; m < depth.rows; m++)
		for (int n = 0; n < depth.cols; n++)
		{
			// 获取深度图中(m,n)处的值
			ushort d = depth.at<ushort>(m,n);
			// d 可能没有值，若如此，跳过此点
			if (d == 0)
				continue;
			// d 存在值，则向点云增加一个点
			pcl::PointXYZRGB p;

			// 计算这个点的空间坐标
			p.z = double(d) / camera_factor;
			p.x = (n - camera_cx) * p.z / camera_fx;
			p.y = (m - camera_cy) * p.z / camera_fy;

			if( flag){
				// 从rgb图像中获取它的颜色
				// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
				p.b = rgb.at<cv::Vec3b>(m,n)[0];
				p.g = rgb.at<cv::Vec3b>(m,n)[1];
				p.r = rgb.at<cv::Vec3b>(m,n)[2];
				// p.a = 255;
				
			}else{
				p.b = 255;
				p.g = 255;
				p.r = 0;
			}
			// std::cout <<std::endl;
	
			// 把p加入到点云中
			cloud->points.push_back(p);
		}


	// 设置并保存点云   无序点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	cout << "point cloud size = " << cloud->points.size() << endl;
	cloud->is_dense = false;
	// 清除数据并退出
	return 0;
}


/**
 * 把点云 可视化
 */
int PclViewTools::VisualPCDTool(const string &inputPath)
{

	PointCloud::Ptr cloud(new PointCloud);
	//read pcd
	int rtn = pcl::io::loadPCDFile(inputPath, *cloud);
	assert(rtn == 0);

	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
	viewer.showCloud(cloud);
	// viewer.runOnVisualizationThreadOnce(viewerOneOff);
	while (!viewer.wasStopped())
		;
	system("pause");

	return 0;
}

/**
 * 载入 stl 文件 查看点云
 */
int PclViewTools::readSTLModel(const string &path)
{
	// pcl::io::loadPolygonFileSTL(path, );
}


/**
 * 点云虚拟化
 */
int PclViewTools::Visual3DTool(const PointCloud::Ptr& pcloud){
	
	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
	viewer.showCloud(pcloud);
	// viewer.runOnVisualizationThreadOnce(viewerOneOff);
	while (!viewer.wasStopped())
		;
	system("pause");

	return 0;
}


// int PclViewTools::LoadPLYModel(PointCloud::Ptr& pcloud, const string & path)
// {
// 	// if (pcl::io::loadPLYFile<pcl::PointXYZ>(path, pcloud) == -1) //* load the file 
//     // {
//     //     PCL_ERROR("Couldn't read file test_pcd.pcd \n");
//     //     system("PAUSE");
//     //     return (-1);
// 	// }

// 	return 0;
// }